/* Author: Binh Cao */
#ifndef CAMERAPLANNER_H_
#define CAMERAPLANNER_H_
#include "ompl/geometric/planners/PlannerIncludes.h" 
#include <vector>

namespace flyingcamera
{
    namespace base
    {
    	namespace moving
    	{
        /**
           @par Short description

        */

			class CameraPlanner : public ompl::base::Planner
			{
				public:

				/** \brief Constructor */
				CameraPlanner(const ompl::base::SpaceInformationPtr &si);

				virtual ~CameraPlanner(void);

				virtual void getPlannerData(ompl::base::PlannerData &data) const;

				virtual ompl::base::PlannerStatus solve(const ompl::base::PlannerTerminationCondition &ptc);

				ompl::base::PlannerStatus solve(double time)
				{
					return Planner::solve(time);
				}

				virtual void clear(void);

				virtual void setup(void);

				bool isSolutionFound() const;

				double getTime() const;

				protected:


				/** \brief Representation of a motion

					This only contains pointers to parent motions as we
					only need to go backwards in the tree. */
				class Motion
				{
				public:

					Motion(void) : state(NULL), parent(NULL)
					{
					}

					/** \brief Constructor that allocates memory for the state */
					Motion(const ompl::base::SpaceInformationPtr &si) : state(si->allocState()), parent(NULL)
					{
					}

					~Motion(void)
					{
					}

					/** \brief The state contained by the motion */
					ompl::base::State       *state;

					/** \brief The parent motion in the exploration tree */
					Motion            *parent;

				};

				/** \brief Free the memory allocated by this planner */
				void freeMemory(void);

				/** \brief State sampler */
				ompl::base::ValidStateSamplerPtr                     sampler_;

				/** \brief A datastructure containing the tree of motions */
				std::vector<Motion*> tree_;

				double                                         timeRange_;

				/** \brief The random number generator */
				ompl::RNG                                            rng_;

				/** \brief The most recent goal motion.  Used for PlannerData computation */
				Motion                                         *lastGoalMotion_;

				//
				bool solutionFound_;
			};
    	}
    }
}
#endif
